clear;
x = [0.00000000
0.00000000
0.00000000
0.00000000
0.00000000
0.00000000
0.99944794
-0.02966589
0.00004304
0.01495959
0.00000000
0.00000000
0.00000000
-0.00000014
-0.00000002
0.00000000
];

P = [...
0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000
0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000
0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000
0.00000000,0.00000000,0.00000000,9.02199936,0.00000001,0.07924422,0.00893650,0.29855269,0.01787297,-0.00000299,0.00000000,0.00000000,0.00000003,0.00000089,0.00000005
0.00000000,0.00000000,0.00000000,0.00000001,9.02231884,0.12422507,-0.29856327,0.00893683,-0.00000027,0.00000000,-0.00000299,0.00000000,-0.00000089,0.00000003,0.00000000
0.00000000,0.00000000,0.00000000,0.07924423,0.12422507,0.00341568,-0.00403277,0.00274568,0.00015700,0.00000000,0.00000000,-0.00000299,-0.00000001,0.00000001,0.00000000
0.00000000,0.00000000,0.00000000,0.00893650,-0.29856327,-0.00403277,0.01089785,-0.00000001,0.00001771,0.00000000,0.00000000,0.00000000,-0.00000296,0.00000000,0.00000000
0.00000000,0.00000000,0.00000000,0.29855269,0.00893683,0.00274568,-0.00000001,0.01089750,0.00059145,0.00000000,0.00000000,0.00000000,-0.00000000,-0.00000296,0.00000000
0.00000000,0.00000000,0.00000000,0.01787297,-0.00000027,0.00015700,0.00001771,0.00059145,0.00104437,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,-0.00000299
0.00000000,0.00000000,0.00000000,-0.00000299,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000200,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000
0.00000000,0.00000000,0.00000000,0.00000000,-0.00000299,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000200,0.00000000,0.00000000,0.00000000,0.00000000
0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,-0.00000299,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000200,0.00000000,0.00000000,0.00000000
0.00000000,0.00000000,0.00000000,0.00000003,-0.00000089,-0.00000001,-0.00000296,-0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000200,0.00000000,-0.00000000
0.00000000,0.00000000,0.00000000,0.00000089,0.00000003,0.00000001,0.00000000,-0.00000296,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000200,-0.00000000
0.00000000,0.00000000,0.00000000,0.00000005,0.00000000,0.00000000,0.00000000,0.00000000,-0.00000299,0.00000000,0.00000000,0.00000000,-0.00000000,-0.00000000,0.00000200];

w_t = [-0.02,0.00,0.01]';

%% Correction

flow = [0.00,0.00]';
f = 65;
p_t = x(1:3);
v_t = x(4:6);
q_t = x(7:10);
wb  = x(14:16);

px = p_t(1);
py = p_t(2);
pz = p_t(3);
vx = v_t(1);
vy = v_t(2);
vz = v_t(3);
qw = q_t(1);
qx = q_t(2);
qy = q_t(3);
qz = q_t(4);
%% h(x) - Optical Flow

if (pz<0.1)
    pz = 0.1;
end

syms cx cy cz

fx=65.0;
fy=65.0;

P_f = [fx 0 0; 0 fy 0];
P_x = [0 fx 0; -fy 0 0];


R_c_i = diag([1,-1,-1]);

R = fromqtoR(q_t);

p_c_w = p_t;
v_c_w = v_t;
R_c_w = R*R_c_i;

v_c_c = R_c_i.'*R.'*v_c_w;
w_c_c = R_c_i.'*(w_t-wb);
z_c = p_c_w(3)/R_c_w(3,3);

% measurement model as a function of system states
h_c = -P_f*(v_c_c/z_c) - P_x*w_c_c;
% jacobian of measurement model
%H_c = jacobian(h_c, x);
%X_dx = Qmat(q);
%X_dx = blkdiag(eye(6), X_dx, eye(6));
%H_dx_c = H_c*X_dx;
 
 
H_dx_c = [ 0, 0, -(fx*(vx*(qw^2 + qx^2 - qy^2 - qz^2) + vy*(2*qw*qz + 2*qx*qy) - vz*(2*qw*qy - 2*qx*qz))*(qw^2 - qx^2 - qy^2 + qz^2))/pz^2, (fx*(qw^2 - qx^2 - qy^2 + qz^2)*(qw^2 + qx^2 - qy^2 - qz^2))/pz,          (fx*(2*qw*qz + 2*qx*qy)*(qw^2 - qx^2 - qy^2 + qz^2))/pz, -(fx*(2*qw*qy - 2*qx*qz)*(qw^2 - qx^2 - qy^2 + qz^2))/pz, - (qx*((2*fx*qw*(vx*(qw^2 + qx^2 - qy^2 - qz^2) + vy*(2*qw*qz + 2*qx*qy) - vz*(2*qw*qy - 2*qx*qz)))/pz + (fx*(2*qw*vx - 2*qy*vz + 2*qz*vy)*(qw^2 - qx^2 - qy^2 + qz^2))/pz))/2 - (qw*((2*fx*qx*(vx*(qw^2 + qx^2 - qy^2 - qz^2) + vy*(2*qw*qz + 2*qx*qy) - vz*(2*qw*qy - 2*qx*qz)))/pz - (fx*(2*qx*vx + 2*qy*vy + 2*qz*vz)*(qw^2 - qx^2 - qy^2 + qz^2))/pz))/2 - (qz*((2*fx*qy*(vx*(qw^2 + qx^2 - qy^2 - qz^2) + vy*(2*qw*qz + 2*qx*qy) - vz*(2*qw*qy - 2*qx*qz)))/pz + (fx*(2*qw*vz - 2*qx*vy + 2*qy*vx)*(qw^2 - qx^2 - qy^2 + qz^2))/pz))/2 - (qy*((2*fx*qz*(vx*(qw^2 + qx^2 - qy^2 - qz^2) + vy*(2*qw*qz + 2*qx*qy) - vz*(2*qw*qy - 2*qx*qz)))/pz + (fx*(2*qw*vy + 2*qx*vz - 2*qz*vx)*(qw^2 - qx^2 - qy^2 + qz^2))/pz))/2, (qx*((2*fx*qz*(vx*(qw^2 + qx^2 - qy^2 - qz^2) + vy*(2*qw*qz + 2*qx*qy) - vz*(2*qw*qy - 2*qx*qz)))/pz + (fx*(2*qw*vy + 2*qx*vz - 2*qz*vx)*(qw^2 - qx^2 - qy^2 + qz^2))/pz))/2 - (qy*((2*fx*qw*(vx*(qw^2 + qx^2 - qy^2 - qz^2) + vy*(2*qw*qz + 2*qx*qy) - vz*(2*qw*qy - 2*qx*qz)))/pz + (fx*(2*qw*vx - 2*qy*vz + 2*qz*vy)*(qw^2 - qx^2 - qy^2 + qz^2))/pz))/2 - (qw*((2*fx*qy*(vx*(qw^2 + qx^2 - qy^2 - qz^2) + vy*(2*qw*qz + 2*qx*qy) - vz*(2*qw*qy - 2*qx*qz)))/pz + (fx*(2*qw*vz - 2*qx*vy + 2*qy*vx)*(qw^2 - qx^2 - qy^2 + qz^2))/pz))/2 + (qz*((2*fx*qx*(vx*(qw^2 + qx^2 - qy^2 - qz^2) + vy*(2*qw*qz + 2*qx*qy) - vz*(2*qw*qy - 2*qx*qz)))/pz - (fx*(2*qx*vx + 2*qy*vy + 2*qz*vz)*(qw^2 - qx^2 - qy^2 + qz^2))/pz))/2, (qx*((2*fx*qy*(vx*(qw^2 + qx^2 - qy^2 - qz^2) + vy*(2*qw*qz + 2*qx*qy) - vz*(2*qw*qy - 2*qx*qz)))/pz + (fx*(2*qw*vz - 2*qx*vy + 2*qy*vx)*(qw^2 - qx^2 - qy^2 + qz^2))/pz))/2 + (qw*((2*fx*qz*(vx*(qw^2 + qx^2 - qy^2 - qz^2) + vy*(2*qw*qz + 2*qx*qy) - vz*(2*qw*qy - 2*qx*qz)))/pz + (fx*(2*qw*vy + 2*qx*vz - 2*qz*vx)*(qw^2 - qx^2 - qy^2 + qz^2))/pz))/2 - (qz*((2*fx*qw*(vx*(qw^2 + qx^2 - qy^2 - qz^2) + vy*(2*qw*qz + 2*qx*qy) - vz*(2*qw*qy - 2*qx*qz)))/pz + (fx*(2*qw*vx - 2*qy*vz + 2*qz*vy)*(qw^2 - qx^2 - qy^2 + qz^2))/pz))/2 - (qy*((2*fx*qx*(vx*(qw^2 + qx^2 - qy^2 - qz^2) + vy*(2*qw*qz + 2*qx*qy) - vz*(2*qw*qy - 2*qx*qz)))/pz - (fx*(2*qx*vx + 2*qy*vy + 2*qz*vz)*(qw^2 - qx^2 - qy^2 + qz^2))/pz))/2, 0, 0, 0,   0, -fx, 0;
 0, 0,  (fy*(vy*(qw^2 - qx^2 + qy^2 - qz^2) - vx*(2*qw*qz - 2*qx*qy) + vz*(2*qw*qx + 2*qy*qz))*(qw^2 - qx^2 - qy^2 + qz^2))/pz^2,         (fy*(2*qw*qz - 2*qx*qy)*(qw^2 - qx^2 - qy^2 + qz^2))/pz, -(fy*(qw^2 - qx^2 - qy^2 + qz^2)*(qw^2 - qx^2 + qy^2 - qz^2))/pz, -(fy*(2*qw*qx + 2*qy*qz)*(qw^2 - qx^2 - qy^2 + qz^2))/pz,   (qw*((2*fy*qx*(vy*(qw^2 - qx^2 + qy^2 - qz^2) - vx*(2*qw*qz - 2*qx*qy) + vz*(2*qw*qx + 2*qy*qz)))/pz - (fy*(2*qw*vz - 2*qx*vy + 2*qy*vx)*(qw^2 - qx^2 - qy^2 + qz^2))/pz))/2 + (qx*((2*fy*qw*(vy*(qw^2 - qx^2 + qy^2 - qz^2) - vx*(2*qw*qz - 2*qx*qy) + vz*(2*qw*qx + 2*qy*qz)))/pz + (fy*(2*qw*vy + 2*qx*vz - 2*qz*vx)*(qw^2 - qx^2 - qy^2 + qz^2))/pz))/2 + (qy*((2*fy*qz*(vy*(qw^2 - qx^2 + qy^2 - qz^2) - vx*(2*qw*qz - 2*qx*qy) + vz*(2*qw*qx + 2*qy*qz)))/pz - (fy*(2*qw*vx - 2*qy*vz + 2*qz*vy)*(qw^2 - qx^2 - qy^2 + qz^2))/pz))/2 + (qz*((2*fy*qy*(vy*(qw^2 - qx^2 + qy^2 - qz^2) - vx*(2*qw*qz - 2*qx*qy) + vz*(2*qw*qx + 2*qy*qz)))/pz - (fy*(2*qx*vx + 2*qy*vy + 2*qz*vz)*(qw^2 - qx^2 - qy^2 + qz^2))/pz))/2, (qy*((2*fy*qw*(vy*(qw^2 - qx^2 + qy^2 - qz^2) - vx*(2*qw*qz - 2*qx*qy) + vz*(2*qw*qx + 2*qy*qz)))/pz + (fy*(2*qw*vy + 2*qx*vz - 2*qz*vx)*(qw^2 - qx^2 - qy^2 + qz^2))/pz))/2 - (qz*((2*fy*qx*(vy*(qw^2 - qx^2 + qy^2 - qz^2) - vx*(2*qw*qz - 2*qx*qy) + vz*(2*qw*qx + 2*qy*qz)))/pz - (fy*(2*qw*vz - 2*qx*vy + 2*qy*vx)*(qw^2 - qx^2 - qy^2 + qz^2))/pz))/2 + (qw*((2*fy*qy*(vy*(qw^2 - qx^2 + qy^2 - qz^2) - vx*(2*qw*qz - 2*qx*qy) + vz*(2*qw*qx + 2*qy*qz)))/pz - (fy*(2*qx*vx + 2*qy*vy + 2*qz*vz)*(qw^2 - qx^2 - qy^2 + qz^2))/pz))/2 - (qx*((2*fy*qz*(vy*(qw^2 - qx^2 + qy^2 - qz^2) - vx*(2*qw*qz - 2*qx*qy) + vz*(2*qw*qx + 2*qy*qz)))/pz - (fy*(2*qw*vx - 2*qy*vz + 2*qz*vy)*(qw^2 - qx^2 - qy^2 + qz^2))/pz))/2, (qy*((2*fy*qx*(vy*(qw^2 - qx^2 + qy^2 - qz^2) - vx*(2*qw*qz - 2*qx*qy) + vz*(2*qw*qx + 2*qy*qz)))/pz - (fy*(2*qw*vz - 2*qx*vy + 2*qy*vx)*(qw^2 - qx^2 - qy^2 + qz^2))/pz))/2 + (qz*((2*fy*qw*(vy*(qw^2 - qx^2 + qy^2 - qz^2) - vx*(2*qw*qz - 2*qx*qy) + vz*(2*qw*qx + 2*qy*qz)))/pz + (fy*(2*qw*vy + 2*qx*vz - 2*qz*vx)*(qw^2 - qx^2 - qy^2 + qz^2))/pz))/2 - (qw*((2*fy*qz*(vy*(qw^2 - qx^2 + qy^2 - qz^2) - vx*(2*qw*qz - 2*qx*qy) + vz*(2*qw*qx + 2*qy*qz)))/pz - (fy*(2*qw*vx - 2*qy*vz + 2*qz*vy)*(qw^2 - qx^2 - qy^2 + qz^2))/pz))/2 - (qx*((2*fy*qy*(vy*(qw^2 - qx^2 + qy^2 - qz^2) - vx*(2*qw*qz - 2*qx*qy) + vz*(2*qw*qx + 2*qy*qz)))/pz - (fy*(2*qx*vx + 2*qy*vy + 2*qz*vz)*(qw^2 - qx^2 - qy^2 + qz^2))/pz))/2, 0, 0, 0, -fy,   0, 0];
 
Z = H_dx_c*P*H_dx_c' + diag([3 3]);
K = P*H_dx_c'/Z;
S = eye(15) - K*H_dx_c;
P = S*P*S' + K*diag([3 3])*K';
